//
// Created by sxy on 2021/11/8.
//

#ifndef LIDAR_LOCALIZATION_MAP_UPDATE_H
#define LIDAR_LOCALIZATION_MAP_UPDATE_H

#include <iostream>
#include <string>
#include <fstream>
#include "CSF.h"
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/octree/octree.h>
#include <vector>
using namespace std;

//地面点删除相关
//std::string cfg_params="";
//用来切割地图的矩形大小，通过len来控制

class Cfg
{
public:
    bool readConfigFile(string cfgfilepath, const std::string & key, std::string & value)
    {
        std::fstream cfgFile;
        cfgFile.open(cfgfilepath);
        if (!cfgFile.is_open())
        {
            std::cout << "can not open cfg file!" << std::endl;
            return false;
        }
        char tmp[1000];
        while (!cfgFile.eof())
        {
            cfgFile.getline(tmp, 1000);
            std::string line(tmp);
            std::size_t pos = line.find('=');
            if (pos == std::string::npos) return false;
            std::string tmpKey = line.substr(0, pos);
            if (key == tmpKey)
            {
                value = line.substr(pos + 1);
                return true;
            }
        }
        return false;
    }

public:
    std::string slop_smooth;
    std::string class_threshold;
    std::string cloth_resolution;
    std::string iterations;
    std::string rigidness;
    std::string time_step;
    std::string terr_pointClouds_filepath;

    CSF csf;

};

//读取配置参数
void readCSGconfig(Cfg &cfg,std::string cfg_parm);

//删除地面点云
void deletegroud(Cfg cfg,pcl::PointCloud<pcl::PointXYZ>::Ptr mapCloud,pcl::PointCloud<pcl::PointXYZ>::Ptr withoutground,pcl::PointCloud<pcl::PointXYZ>::Ptr ground);

//配准两个点云
void registration(pcl::PointCloud<pcl::PointXYZ>::Ptr source,pcl::PointCloud<pcl::PointXYZ>::Ptr target,Eigen::Matrix4f& transform);

//增量式检测点云变化
void detect_change(pcl::PointCloud<pcl::PointXYZ>::Ptr input_origin,pcl::PointCloud<pcl::PointXYZ>::Ptr input_add,std::vector<int>& index);


void rectangle(double len,vector<double>& leftup,vector<double>& rightup,vector<double>& rightdown,vector<double>& leftdown,double x1,double y1,double x2,double y2);
























#endif //LIDAR_LOCALIZATION_MAP_UPDATE_H
